package org.discobots.practice;


import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class PracticeRobot extends IterativeRobot {
    RobotDrive drive;
    GamePad gp1;
    long time=0;
    double move=0, turn=0;
//    private Timer timer=new Timer();
//    Thread test=new Thread(){
//            public void run(){
//                while(true){
//                    for(int i=1;i<=gp1.getNumButtons();i++)
//                    System.out.println(i+": "+gp1.getRawButton(i));
//                    try {
//                        Thread.sleep(1000);
//                    } catch (InterruptedException ex) {
//                        ex.printStackTrace();
//                    }
//                }
//            }
//        };


    public void robotInit() {
        gp1=new GamePad(1,GamePad.MODE_D);

        drive= new RobotDrive(  HW.leftFrontDriveChannel,
                                HW.leftBackDriveChannel,
                                HW.rightFrontDriveChannel,
                                HW.rightBackDriveChannel);
        //test.run();
    }

    public void disabledInit(){
	drive.arcadeDrive(0,0);

    }

    public void disabledContinuous(){
	drive.arcadeDrive(0,0);
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {

    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        System.out.println(move);
        System.out.println(turn);
        System.out.println(System.currentTimeMillis()-time);
        time=System.currentTimeMillis();
    }

    public void teleopContinuous(){
        
        move=gp1.getRawAxis(gp1.AXIS_LX);
        turn=gp1.getRawAxis(gp1.AXIS_LY);
        drive.arcadeDrive(move,turn,true);
//        drive.tankDrive(move+turn, move-turn);
        
    }

}
